
#include "animator.h"
#include "quaternion.h"

Animator::Animator(MeshWithSkeleton *me, Motion *m):
   mesh(me), motion(m), framescursor(-1) {
   
   setting();
   
}

void Animator::next() {
   
   ++framescursor;
   /// skeleton animation
   vector<Point3>  &j = mesh->skeleton->joints;
   vector<Vector3> &d = motion->displacements[framescursor];
   for (unsigned i=0; i<mesh->skeleton->nJoints(); ++i) {
      j[i] = j[i] + d[i];
   }
   
   /// project to the mesh
   project_deformation_on_mesh_std_dualquat();
}

void Animator::prev() {
   
   --framescursor;
   vector<Point3>  &j = mesh->skeleton->joints;
   vector<Vector3> &d = motion->displacements[framescursor];
   for (unsigned i=0; i<mesh->skeleton->nJoints(); ++i) {
      j[i] = j[i] - d[i];
   }
   
   /// project to the mesh
   project_deformation_on_mesh_std_dualquat();
}

void Animator::last() {
   
   int limit = motion->size()-1;
   if (limit >= framescursor) {
      do {
         next();
      } while (framescursor < limit);
   }
}

void Animator::first() {
   
   recoverInitData();
   framescursor = -1;
}

void Animator::frame(int frameid) {
   
   // frameid==-1 means to set the mesh on its original position 
   if (frameid < 0) { 
      first(); 
      return;
   }
   
   // frameid isnot an valid index
   if (frameid >= (int)(motion->size()-1)) return; 
   
   if (frameid < framescursor) {
      do {
         prev();
      } while (framescursor > frameid);
   }
   else if (frameid > framescursor) {
      do {
         next();
      } while (framescursor < frameid);
   }
   // if (f == framescursor) dont animate
}

void Animator::setting() {
   
   assert(mesh != 0);
   assert(mesh->skeleton != 0);
   
   ipositions.resize(mesh->nvertices());
   inormals.resize(mesh->nvertices());
   Vertex_iterator vi = mesh->vertices_begin();
   for (unsigned int i=0; i<ipositions.size(); ++i, ++vi) {
      ipositions[i] = vi->point();
      inormals[i] = vi->normal();
   }
   
   old_angle.clear();
   old_angle.resize(mesh->skeleton->nBones());
   for (int i=0; i<old_angle.size(); ++i) {
      vector<Number> array(mesh->skeleton->nBones(), 0.0);
      old_angle[i] = array;
   }
   
   cout<<"setting end"<<endl;
   turns.clear();
   turns.resize(mesh->skeleton->nBones(), 1);
   
   ubones.resize(18, Vector3(0,0,0));
   //~ tbones.resize(18, Vector3(0,0,0));
   
   tvectors0.resize(18, Vector3(0,0,0));
   tvectors1.resize(18, Vector3(0,0,0));
   tscalar0.resize(18,0);
   tscalar1.resize(18,0);
   
   tlvectors0.resize(18, Vector3(0,0,0));
   tlvectors1.resize(18, Vector3(0,0,0));
}

inline Number SGN(Number n) { if (n<0) return -1; else return 1;}

void Animator::project_deformation_on_mesh_std_dualquat() {
   
   //< skip the first bone
   ubones[0] = Vector3(0,0,0);
   tvectors0[0] = Vector3(0,0,0);
   tvectors1[0] = Vector3(0,0,0);
   tscalar0[0] = 0;
   tscalar1[0] = 0;
   
   tlvectors0[0] = Vector3(0,0,0);
   tlvectors1[0] = Vector3(0,0,0);
   
   for (unsigned int ii=1; ii<mesh->nBones(); ++ii) {
      
      int i = mesh->i_skeleton->orderList[ii];
      
      Vector3 oldu = ubones[i];
      comprotaround = true;
      getRotationRep(i); // compute the ubones[i]
      
      if (comprotaround) {
         
         Vector3 urot(0,0,0);
         urot = getRotationAroundBoneRep(i); 
      
         //~ if (abs(1.0-ubones[i]*urot)<1e-8) {
            //~ Vector3 unitu = (ubones[i]+urot+cross_product(ubones[i],urot));
            //~ Number nunitu = sqrt(unitu*unitu);
            //~ if (nunitu < 1e-8)
               //~ ubones[i] = oldu;
            //~ else {
               //~ unitu = unitu/nunitu;
               //~ ubones[i] = unitu*(SGN(oldu*unitu)*1e8);
            //~ }
         //~ }
         //~ else {
            //~ ubones[i] = (ubones[i]+urot+cross_product(ubones[i],urot))/(1-ubones[i]*urot);
         //~ }
         
         /// another method
         Number cosbonei = 1.0/sqrt(1+ubones[i]*ubones[i]);
         Vector3 vecbonei = ubones[i]*cosbonei;
         
         Number cosroti = 1.0/sqrt(1+urot*urot);
         Vector3 vecroti = urot*cosroti;
         
         Number cosprod = cosbonei*cosroti - vecbonei*vecroti;
         Vector3 vecprod = vecroti*cosbonei + vecbonei*cosroti + cross_product(vecbonei,vecroti);
         if (cosprod<0) { 
            cosprod = -cosprod;
            vecprod = vecprod*(-1);
         }
         
         if (cosprod < 0.01)  cosprod = 0.01;
         ubones[i] = vecprod/cosprod;
      }
      else {
         //cout<<"else comprotaround = true"<<endl;
         comprotaround = true;
      }
   }
   
   Skeleton *si = mesh->i_skeleton; // initial skeleton
   for (unsigned int i=1; i<mesh->nBones(); ++i) {
      
      Skeleton *si = mesh->i_skeleton; // initial skeleton
      Skeleton *sd = mesh->skeleton;   // deformed skeleton
      const Bone &b = si->bones[i];   // initial bone
      const Point3 &p0 = si->joints[b.first]; 
      const Point3 &p1 = si->joints[b.second];
      const Point3 &q0 = sd->joints[b.first]; 
      const Point3 &q1 = sd->joints[b.second];
      
      Number nubone2 = ubones[i]*ubones[i];
      Number denom = 1.0/sqrt(1.0 + nubone2);
      Vector3 utemp = ubones[i]*denom;
      u[0] = ubones[i][0]; u[1] = ubones[i][1]; u[2] = ubones[i][2];
      computeRotation(&p0[0]);
      
      Vector3 t0 = q0 - Point3(rv[0],rv[1],rv[2]);
      tscalar0[i] = -1.0*(t0*utemp);
      tlvectors0[i] = t0;
      tvectors0[i] = (t0*denom - cross_product(utemp,t0));
      
      computeRotation(&p1[0]);
      Vector3 t1 = q1 - Point3(rv[0],rv[1],rv[2]);
      tscalar1[i] = -1.0*(t1*utemp);
      tlvectors1[i] = t1;
      tvectors1[i] = (t1*denom - cross_product(utemp,t1));
   }
   
   //~ cout<<"projectando ... "<<endl;
   /// on vertices
   unsigned int ii=0, iv=0;
   Vertex_iterator vi = mesh->vertices_begin();
   while (vi != mesh->vertices_end()) {
      vector<Vector3> vt(vi->bonesweights.size(), Vector3(0,0,0));
      vector<Number> sc(vi->bonesweights.size(), 0);
      Point3 po = vi->point();
      
      Number sumweights = 0;
      
      Number s1 = 0;
      Vector3 t1(0,0,0);
      Number qs1 = 0;
      Vector3 qv1(0,0,0);
      
      Vector3 vtl(0,0,0);
      
      Vector3 u1(0,0,0);
      Vector3 vmod(0,0,0); // position
      Vector3 nmod(0,0,0); // normal
      for (unsigned int i=1; i<vi->bonesweights.size(); ++i) {
         if (vi->bonesweights[i] == 0) continue;
         
         vtl = (tlvectors0[i])*(vi->ts[i]) + (tlvectors1[i])*(1.0-vi->ts[i]);
         vt[i] = (tvectors0[i])*(vi->ts[i]) + (tvectors1[i])*(1.0-vi->ts[i]);
         sc[i] = (tscalar0[i])*(vi->ts[i])  + (tscalar1[i])*(1.0-vi->ts[i]);
         
         /// using ?
         u[0]=ubones[i][0]; u[1]=ubones[i][1]; u[2]=ubones[i][2];
         computeRotation(&ipositions[ii][0]);
         vmod = vmod + (Vector3(rv[0],rv[1],rv[2]) + vtl)*vi->bonesweights[i];
         
         computeRotation(&inormals[ii][0]);
         nmod = nmod + (Vector3(rv[0],rv[1],rv[2]))*vi->bonesweights[i];
         
         /// using quat
         Number cosi = (sqrt(1.0/(1.0+(ubones[i])*(ubones[i])))); //turns[i];
         Vector3 vecparti = ubones[i]*cosi;
         
         if ((qs1*cosi + qv1*vecparti)<0) {
            cosi = -cosi;
            vecparti = -vecparti;
            vt[i] = vt[i]*(-1.0);
            sc[i] = sc[i]*(-1.0);
            //~ cout<<"changed ..."<<endl;
         }
         s1 += sc[i]*vi->bonesweights[i];
         t1 = t1 + vt[i]*vi->bonesweights[i];
         qs1 += cosi*vi->bonesweights[i];
         qv1 = qv1 + (vecparti)*vi->bonesweights[i];
         
         sumweights += vi->bonesweights[i];
      }
      
      vmod = vmod/sumweights;
      nmod = nmod/sumweights;
      s1 /= sumweights;
      t1 = t1/sumweights;
      qs1 /= sumweights;
      qv1 = qv1/sumweights;
      
      Number normaq = sqrt(qs1*qs1 + qv1*qv1);
      assert(normaq != 0);
      qs1 = qs1/normaq;
      qv1 = qv1/normaq;
      s1 = s1/normaq;
      t1 = t1/normaq;
      
      Vector3 tv = t1*qs1 - qv1*s1 + cross_product(qv1, t1);
      //~ Vector3 tv = t1;
      
      /// 1) Using the u rotation representation
      // Rotate the vertex position
      u[0] = qv1[0]*(1.0/qs1); u[1] = qv1[1]*(1.0/qs1); u[2] = qv1[2]*(1.0/qs1);
      computeRotation(&ipositions[ii][0]);
      Point3 &p = vi->point();
      p = Point3(tv[0]+rv[0], tv[1]+rv[1], tv[2]+rv[2]);
      // Rotate the normal vector
      computeRotation(&inormals[ii][0]);
      Vector3 &n = vi->normal();
      n = Vector3(rv[0], rv[1], rv[2]);
      
      /// 2) Using the ??? method
      //~ // Update the vertex position
      //~ Point3 &p = vi->point();
      //~ p = Point3(vmod[0],vmod[1],vmod[2]);
      //~ // Upate the normal vector
      //~ Vector3 &n = vi->normal();
      //~ n = Vector3(nmod[0], nmod[1], nmod[2]);
      
      /// 3) Using the quaternion method
      //~ typedef Quaternion<Number> Quat;
      //~ Quat Q(qs1, qv1[0],qv1[1],qv1[2]);
      //~ // Rotate the vertex position
      //~ const Point3 &Po = ipositions[ii];
      //~ Quat qPo(Po[0],Po[1],Po[2]);
      //~ Quat qp = Q*qPo*conjugate(Q);
      //~ Point3 &P = vi->point();
      //~ P = Point3(qp[1],qp[2],qp[3]) + tv; // the deformed vertex position
      //~ // Rotate the normal vector
      //~ const Vector3 &No = inormals[ii];
      //~ Quat qNo(No[0],No[1],No[2]);
      //~ Quat qn = Q*qNo*conjugate(Q);
      //~ Vector3 &n = vi->normal();
      //~ n = Vector3(qn[1], qn[2], qn[3]);
      
      /// ////////////////////
      ++vi; ++ii; iv+=3;
   }
}

void Animator::computeRotation(const Number v[]) {
   
   /// Perform the rotation of v
   vxu[0] = v[1]*u[2]-v[2]*u[1];
   vxu[1] = v[2]*u[0]-v[0]*u[2];
   vxu[2] = v[0]*u[1]-v[1]*u[0];
   
   factor = -2.0/(1.0 + u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
   rv[0] = u[1]*vxu[2] - u[2]*vxu[1] + vxu[0];
   rv[1] = u[2]*vxu[0] - u[0]*vxu[2] + vxu[1];
   rv[2] = u[0]*vxu[1] - u[1]*vxu[0] + vxu[2];
   
   rv[0] = v[0] + factor*rv[0];
   rv[1] = v[1] + factor*rv[1];
   rv[2] = v[2] + factor*rv[2];
}

Vector3 Animator::getRotationAroundBoneRep(unsigned int i) {
   
   Skeleton *si = mesh->i_skeleton; // initial skeleton
   Skeleton *sd = mesh->skeleton;   // deformed skeleton
   
   const Bone &bone = si->bones[i];   // initial bone
   Vector3 vbone = si->joints[bone.second] - si->joints[bone.first]; // direction vector of initial bone 
   vbone = vbone/sqrt(vbone*vbone); // normalize
   Vector3 vbonerot = sd->joints[bone.second] - sd->joints[bone.first];
   vbonerot = vbonerot/sqrt(vbonerot*vbonerot); // normalize
   
   Number normau = 0;
   Number angle=0, sumangle=0;
   Number sumw=0;
   Number w = 1.0/si->controller[i].size();
   
   for (unsigned int ii=0; ii<si->controller[i].size(); ++ii) {
      
      Vector3 di; Vector3 ddorig;
      int j = si->controller[i][ii];
      if (j>si->bones.size()) {
         j = j-si->bones.size();
         di = si->atbones[j];
         u[0] = ubones[j][0]; u[1] = ubones[j][1]; u[2] = ubones[j][2];
         computeRotation(&di[0]);
         ddorig = Vector3(rv[0], rv[1], rv[2]);
      }
      else {
         const Bone &bi = si->bones[j];   // initial bone
         const Bone &bd = sd->bones[j];   // deformed bone
         di = si->joints[bi.second] - si->joints[bi.first]; // direction vector of initial bone 
         ddorig = sd->joints[bd.second] - sd->joints[bd.first]; // direction vector of deformed bone
      }
      
      //~ u = ubones[i]*(-1.0);
      u[0] = ubones[i][0]*(-1.0); u[1] = ubones[i][1]*(-1.0); u[2] = ubones[i][2]*(-1.0);
      computeRotation(&ddorig[0]);
      Vector3 dd(rv[0], rv[1], rv[2]);
      
      Vector3 unidi = di/sqrt(di*di);
      Vector3 unidd = dd/sqrt(dd*dd);
      
      /// 1er metodo
      Vector3 bisectriz = (unidi+unidd)*0.5;
      Number normab = sqrt(bisectriz*bisectriz);
      
      if (normab != 0) {
         bisectriz = bisectriz*(1.0/normab);
         
         Number pdot = unidi*unidd;
         if (pdot > 1.0) pdot = 1.0;
         if (pdot <-1.0) pdot =-1.0;
         Number angleini = acos(pdot);
         
         Number normav = tan(angleini/2.0);
         Vector3 u0 = cross_product(unidi,unidd);
         if (u0*u0 < 1e-8) { u0=Vector3(0,0,0); cout<<"u0=0"<<endl;}
         else              u0 = (u0/(sqrt(u0*u0)))*normav;
         Vector3 u1 = cross_product(bisectriz,vbone);
         Vector3 u2 = cross_product(u1,bisectriz);
         Number den = u2*vbone;
         if (abs(den)>1e-3) {
            
            normau = (u2*u0)/den;
            angle = 2*atan(normau);
            //~ if (abs(normau)>0.5) {
               if (i==15) { /*
                  cout<<">>> pdot: "<<pdot<<" : "<<bisectriz<<" : "<<angleini<<endl;
                  cout<<"  normav: "<<normav<<endl;
                  cout<<"  u0: "<<u0<<" : "<<u1<<" : "<<u2<<" : "<<den<<endl;
                  cout<<"  normau: "<<normau<<endl;
                  cout<<"  unidi,unidd: "<<unidi<<" : "<<unidd<<endl;
                  */
               }
            //~ }
         }
         else {
            cout<<"else else else else else else 11111111111111"<<endl;
            if (abs(u2*u0) < 1e-3) {
               cout<<"if abs(u2*u0) < 1e-3"<<endl;
               angle = old_angle[i][j];
               normau = tan(angle/2.0);
            }
            else {
               cout<<"else else else else else else else else else "<<endl;
               if (abs(den)<1e-8) {
                  angle = old_angle[i][j];
                  normau = tan(angle/2.0);
                  //~ normau = SGN(normau)*1e8;
                  cout<<"abs(den)<1e-8 abs(den)<1e-8"<<endl;
               }
               else {
                  cout<<"else else 222222222222"<<endl;
                  normau = (u2*u0)/den;
                  angle = 2*atan(normau);
               }
            }
         }
      }
      else {
         cout<<"else else 3333333333333"<<endl;
         angle = old_angle[i][j];
         normau = tan(angle/2.0);
      }
      
      
      /// ////// 2do metodo
      //~ di = di - vbone*(di*vbone);
      //~ dd = dd - vbone*(dd*vbone);
      //~ Number ndi = sqrt(di*di);
      //~ Number ndd = sqrt(dd*dd);
      
      //~ if (ndi*ndd < 1e-3) {
         //~ angle = old_angle[i][j];
         //~ //continue;
      //~ }
      //~ else {
         //~ di =di/ndi;
         //~ dd =dd/ndd;
         //~ Vector3 dixdd = cross_product(di,dd);
         //~ if ((dixdd * dixdd) <1.0e-3) {
            //~ angle = old_angle[i][j];
         //~ }
         //~ else {
            //~ Number pdot = di *dd;
            //~ if (pdot > 1.0) pdot = 1.0;
            //~ if (pdot < -1.0) pdot = -1.0;
            //~ angle = acos(pdot);
            //~ if(dixdd *vbone < 0) angle =-angle;
         //~ }
      //~ }
      //~ normau = tan(angle/2.0);
      /// //////////// 
      
      /// ///
      //~ Number dif_angle = angle-old_angle[i][j];
      //~ Number turns_dif;
      //~ if (dif_angle < 0) turns_dif = -1;
      //~ else               turns_dif = 0;
      
      //~ turns_dif += (int)(dif_angle/6.2832);
      //~ angle -= turns_dif*6.2832;
      
      //~ if ((angle-old_angle[i][j])>3.1416)
         //~ angle -= 6.2832;
         
      //~ if (numframe>1) {
         //~ if (abs(angle-old_angle[i][j])>0.5)
            //~ angle = old_angle[i][j];
      //~ }
      
      /// ///
      old_angle[i][j] = angle;
      
      if (si->manOrAutowab[i]) {
         w = si->wab[i][ii];
      }
      
      sumangle +=  w*angle;
      sumw += w;
   }
   
   if (sumw==0) { sumangle = 0; }
   else sumangle /= sumw;
   
   //~ if (abs(sumangle) > 6.2832) sumangle -= ((int)(sumangle/6.2832))*6.2832;
   //~ if (sumangle > 3.1416)      sumangle -= 6.2832;
   //~ if (sumangle < -3.1416)     sumangle += 6.2832;
   
   sumangle = sumangle/2.0;
   normau = tan(sumangle);
   
   return vbone*normau;
}

void Animator::getRotationRep(unsigned int i) {
   
   Skeleton *si = mesh->i_skeleton; // initial skeleton
   Skeleton *sd = mesh->skeleton;   // deformed skeleton
   const Bone &bi = si->bones[i];   // initial bone
   const Bone &bd = sd->bones[i];   // deformed bone
   
   Vector3 di = si->joints[bi.second] - si->joints[bi.first]; // direction vector of initial bone 
   Vector3 dd = sd->joints[bd.second] - sd->joints[bd.first]; // direction vector of deformed bone 
   
   di = di/sqrt(di*di); // normalize
   dd = dd/sqrt(dd*dd); // normalize
   
   Vector3 a = cross_product(di,dd);
   if (a*a < 1.0e-4) { // bone i collinear with the initial bone i
      comprotaround=false; 
      return; 
   }
   
   Vector3 bisectriz = (di+dd)*0.5;
   Number normab = sqrt(bisectriz*bisectriz);
   bisectriz = bisectriz/(normab);
   ubones[i] = bisectriz*(ubones[i]*bisectriz);
   
   Number pdot = di*dd;
   if (pdot >= 1.0) pdot = 1.0; // numerical error
   
   Number normau;
   a =a/sqrt(a*a);
   if(pdot <= -1.0) normau = 1.0e8; // numerical error or rotation of 180 degrees
   else normau = sqrt((1.0-pdot)/(1.0+pdot)); 
   
   ubones[i] = ubones[i] + a*normau;
}

void Animator::recoverInitData() {
   
   /// Recover the mesh geometry
   Vertex_iterator vi = mesh->vertices_begin(); int i=0;
   while (vi != mesh->vertices_end()) {
      Point3 &p = vi->point();
      Vector3 &n = vi->normal();
      p = ipositions[i];
      n = inormals[i];
      ++vi; ++i;
   }
   
   /// Recover the joints geometry
   mesh->skeleton->joints = mesh->i_skeleton->joints;
}
